# -*- encoding: utf-8 -*-
"""
Edited at 2018-6-20  by Stone
    1. Add CrossRoad.laneGuides and its show and  hide method.
"""
__metaclass__ = object
from Shapes import *
from visual import *

# Type definition
pos = vector  # to convenient some pos calculation


# Classes
class Car( VehicleShape):
    """This class is used to provide the geometric and dynamic features of the Cars.
    All the unit is based on SI, i.e. m/s, m/s^2"""
    def __init__(self, axis, driver=None):
        super(Car, self).__init__(self, model='car')
        print 11111111111111111111111111111111111111, dir(self)
        self.__axis = axis  # the orientation
        self.__body = super(Car, self).__body
        self.__body.axis = self.__axis
        self.__pos  = None
        self.__v = None
        self.__a = None
        self.__w = None  # omega, angular velocity

        self.driver = driver
        self.give_a_driver(driver)

    def move_one_frame(self, delta_t):
        self.__pos += self.__v * delta_t
        self.__v   += self.__a * delta_t
        self.__axis*= self.__v

    def get_pos(self):
        return self.__pos

    def get_axis(self):
        return self.__axis

    def get_a(self):
        return self.__a

    def get_v(self):
        return self.__v

    def get_w(self):
        return self.__w

    def get_face_to_face_distance(self, anotherCar):
        """To get the distance from the former car's back face to this car's front face."""
        l, w, h = anotherCar.get_size()
        delta_pos = anotherCar.get_pos() - self.__pos
        dis = sqrt(delta_pos[0]**2 + delta_pos[1]**2 + delta_pos[2]**2) - (l + self.__length)/2

        return dis

    def set_pos(self, pos):
        self.__pos = pos

    def set_a(self, a):
        self.__a = a

    def set_axis(self, axis):
        """The axis should be visual.vector type"""
        self.__axis = axis

    def is_it_your_driver(self, theDriver):
        return self.driver == theDriver

    def give_a_driver(self, driver):
        self.driver = driver
        if self.driver and (not self.driver.is_it_your_car(self) ):
            self.driver.give_a_car(self)


class CrossRoad(CrossRoadShape):
    """Construct the shape of the crossroad and provide some attributes of this road."""
    def __init__(self, visual_frame):
        self.f = visual_frame
        CrossRoadShape.__init__(self, self.f)

        self.ifShow_LaneGuides = True
        self.laneGuides = {'x': [], '-x': [], 'y': [], '-y': [], 'visual': []}  # used to guide the vehicles
        self.start = {'x': [], '-x': [], 'y': [], '-y': [], 'visual': []}  # where the vehicles start

        self.init_laneGuides()
        if self.ifShow_LaneGuides: self.show_laneGuides()
        self.construct_shape()

    def init_laneGuides(self):
        def r(v): return -v[1], v[0], v[2]                          # rotate 90 degree
        num_of_lanes = 4
        gap = self.width / num_of_lanes
        direction = vector(1,0,0)
        # not clever enough, so manually specify some values intelligently
        k=2
        for i in range(1, num_of_lanes + 1):
            self.start['x'].append(( -k * self.length,  i * self.width / (num_of_lanes + 1), 0))
            self.start['-x'].append(( k * self.length, -i * self.width / (num_of_lanes + 1), 0))
            self.start['y'].append((  i * self.width / (num_of_lanes + 1), -k * self.length, 0))
            self.start['-y'].append((-i * self.width / (num_of_lanes + 1),  k * self.length, 0))

            #the lanes that go straight
            self.laneGuides[ 'x'].append([( k*self.length, i*self.width/ (num_of_lanes+1), 0)])  # the guide point
            self.laneGuides['-x'].append([(-k*self.length,-i*self.width/ (num_of_lanes+1), 0)])  # -x direction
            self.laneGuides[ 'y'].append([(i*self.width/ (num_of_lanes+1),  k*self.length, 0)])  # y direction, too
            self.laneGuides['-y'].append([(-i*self.width/(num_of_lanes+1), -k*self.length, 0)])

        # the fifth lanes that turn right
        # the middle guide points
        self.laneGuides['x'].append([(self.laneGuides['-y'][num_of_lanes - 1][0][0], i * self.width / (num_of_lanes + 1), 0)])
        self.laneGuides['-x'].append([(self.laneGuides[ 'y'][num_of_lanes - 1][0][0], -i * self.width / (num_of_lanes + 1), 0)])
        self.laneGuides['y'].append([(i * self.width / (num_of_lanes + 1), self.laneGuides[ 'x'][num_of_lanes - 1][0][1], 0)])
        self.laneGuides['-y'].append([(-i * self.width / (num_of_lanes + 1), self.laneGuides['-x'][num_of_lanes - 1][0][1], 0)])
        k = 2
        self.start['x'].append((-k * self.length, i * self.width / (num_of_lanes + 1), 0))
        self.start['-x'].append((k * self.length, -i * self.width / (num_of_lanes + 1), 0))
        self.start['y'].append((i * self.width / (num_of_lanes + 1), -k * self.length, 0))
        self.start['-y'].append((-i * self.width / (num_of_lanes + 1), k * self.length, 0))
        # the end guide points
        self.laneGuides[ 'x'][-1].append((self.laneGuides['-y'][num_of_lanes - 1][0][0], -k*self.length, 0))
        self.laneGuides['-y'][-1].append((-k*self.length, self.laneGuides['-x'][num_of_lanes - 1][0][1], 0))
        self.laneGuides['-x'][-1].append((self.laneGuides[ 'y'][num_of_lanes - 1][0][0],  k*self.length, 0))
        self.laneGuides[ 'y'][-1].append(( k*self.length, self.laneGuides[ 'x'][num_of_lanes - 1][0][1], 0))
        print self.laneGuides

    def hide_laneGuides(self):
        for obj in self.laneGuides['visual']:
            obj.visible = False

    def show_laneGuides(self):
        if self.laneGuides['visual'] == []:  # time to initialize
            for i in self.laneGuides:
                if i == 'visual': continue
                for j in range(len(self.laneGuides[i])):
                    self.laneGuides['visual'].append(curve(pos=[self.start[i][j]] + self.laneGuides[i][j]))
        for obj in self.laneGuides['visual']:
            obj.visible = True


class Driver:
    """Drive the car, which means put the traffic rules in this class."""
    def __init__(self):pass
    def __initu__(self, start, laneGuide, car=None):
        self.__MAX_A = (100.0/3.6) / (10/3600.0)  # (100km/h / 3.6)/10s, max acceleration, expressed in 100km/h speed up
        self.__MAX_V = 60/3.6                     # 60 km/h / 3.6, max velocity
        self.__DISTANCE_RANGE = (0.5, 2)          # the distance range between two cars
        self.__k_a = 0.3
        self.__k_w = 0.5
        self.__target_v = 50
        self.__laneGuide = iter(laneGuide)                   # the laneGuide at specific lane
        self.__node = {'from': vector(start), 'to': vector(laneGuide[0])}   # the last node where it from and the next one where it's going to
        self.__target_orientation = self.get_target_orientation()

        self.car = car
        self.give_a_car(car)

    def drive(self):
        self.control_a()

        self.watch_a()
        self.watch_v()
        self.watch_orientation()
        self.car.move_one_frame()

    def control_a(self):
        delta_v = self.__target_v - self.car.get_v()
        if delta_v > 0:
            self.car.set_a(self.__k_a * delta_v)
        else:
            self.car.set_a(0)

    def update_node(self):
        self.__node['from'] = self.__node['to']
        try:
            self.__node['to']   = vector(self.__laneGuide.next())
        except StopIteration:
            # now we get the destination, time to say bye bye
            self.say_goodbye()

    def get_target_orientation(self):
        return self.__node['to'] - self.__node['from']

    def say_goodbye(self):pass

    def watch_a(self):
        if self.car.get_a > self.__MAX_A:
            self.car.set_a(self.__MAX_A)

    def watch_v(self):
        if self.car.get_v() > self.__MAX_V:
            self.car.set_v(self.__MAX_V)

    def watch_distance(self):
        pass

    def watch_orientation(self):
        w = self.__k_w * (self.__target_orientation - self.car.axis)
        self.car.set_w(w)

    def is_it_your_car(self, theCar):
        return self.car == theCar

    def give_a_car(self, car):
        self.car = car
        if self.car and (not self.car.is_it_your_driver(self)):
            self.car.give_a_driver(self)


# Initialization
scene = display(title=u"十字路口模拟器2018 - by teleM".encode("gbk"))
scene.fov = pi/8
scene.forward = (0, -1, -1)
scene.up = (0, 0, 1)
crd = CrossRoad(frame())

car_groups = dict([(k, []) for k in crd.laneGuides])


# Main Loop
car_groups['x'].append(Car(axis=vector(1, 0, 0), driver=Driver(crd.start['x'][0], crd.laneGuides['x'][0])))  # debug
def mainLoop():
    car_groups['x'][0].drive()
    rate(10)



if __name__ == '__main__':
    while 1:
       mainLoop()
